// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Pose2D.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Numerics
{
    using System;
    using System.Diagnostics;
    using System.Runtime.CompilerServices;
    using System.Runtime.Serialization;

    /// <summary>
    /// Pose class
    /// </summary>
    [DebuggerDisplay("<{x}, {y}, {heading}rad>")]
    [DataContract]
    public struct Pose2D
    {
        /// <summary>
        /// The translation in X
        /// </summary>
        [DataMember]
        private double x;

        /// <summary>
        /// The translation in Y
        /// </summary>
        [DataMember]
        private double y;

        /// <summary>
        /// The orientation
        /// </summary>
        [DataMember]
        private double heading;

        /// <summary>
        /// Initializes a new instance of the Pose2D struct
        /// </summary>
        /// <param name="x">Translation in X</param>
        /// <param name="y">Translation in Y</param>
        /// <param name="heading">The orientation</param>
        public Pose2D(double x, double y, double heading)
        {
            this.x = x;
            this.y = y;
            this.heading = ClampAngle(heading);
        }

        /// <summary>
        /// Initializes a new instance of the Pose2D struct
        /// </summary>
        /// <param name="x">Translation in X</param>
        /// <param name="y">Translation in Y</param>
        public Pose2D(double x, double y)
            : this(x, y, double.NaN)
        {
        }

        /// <summary>
        /// Initializes a new instance of the Pose2D struct.
        /// Projects the specified 3D pose onto the horizontal plane.
        /// </summary>
        /// <param name="pose3D">Pose to project in the horizontal plane</param>
        public Pose2D(Pose pose3D)
            : this(pose3D.Position.X, pose3D.Position.Y, pose3D.GetZYXEulerAngles().Z)
        {
        }

        /// <summary>
        /// Gets or sets the translation in X
        /// </summary>
        public double X
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return this.x;
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            set
            {
                this.x = value;
            }
        }

        /// <summary>
        /// Gets or sets the translation in Y
        /// </summary>
        public double Y
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return this.y;
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            set
            {
                this.y = value;
            }
        }

        /// <summary>
        /// Gets or sets the orientation as an angle in radians
        /// </summary>
        public double Heading
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return this.heading;
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            set
            {
                this.heading = ClampAngle(value);
            }
        }

        /// <summary>
        /// Gets a value indicating whether this pose includes a heading value.
        /// </summary>
        public bool HasHeading
        {
            get { return !double.IsNaN(this.heading); }
        }

        /// <summary>
        /// Clamps the angle value passed in to the [-Pi, Pi] interval
        /// </summary>
        /// <param name="heading">The angle to clamp, in radians</param>
        /// <returns>The corresponding value in the [-Pi, Pi] interval</returns>
        public static double ClampAngle(double heading)
        {
            heading = heading % MathConstants.TwoPI;
            while (heading > Math.PI)
            {
                heading -= MathConstants.TwoPI;
            }

            while (heading < -Math.PI)
            {
                heading += MathConstants.TwoPI;
            }

            return heading;
        }

        /// <summary>
        /// The equals operator
        /// </summary>
        /// <param name="leftSide">Pose on the left side</param>
        /// <param name="rightSide">Pose on the right side</param>
        /// <returns>Are the Pose values equal</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator ==(Pose2D leftSide, Pose2D rightSide)
        {
            // Return true if the fields match:
            return leftSide.heading == rightSide.heading && leftSide.x == rightSide.x && leftSide.y == rightSide.y;
        }

        /// <summary>
        /// The not equals operator
        /// </summary>
        /// <param name="leftSide">Pose on the left side</param>
        /// <param name="rightSide">Pose on the right side</param>
        /// <returns>The not equal result</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator !=(Pose2D leftSide, Pose2D rightSide)
        {
            return !(leftSide == rightSide);
        }

        /// <summary>
        /// Subtracts two poses to obtain a delta from the second to the first
        /// </summary>
        /// <param name="leftSide">First pose</param>
        /// <param name="rightSide">Second pose</param>
        /// <returns>The delta that would generate the second pose when applied to the first.</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Pose2D operator -(Pose2D leftSide, Pose2D rightSide)
        {
            return new Pose2D(leftSide.X - rightSide.X, leftSide.Y - rightSide.Y, leftSide.Heading - rightSide.Heading);
        }

        /// <summary>
        /// Adds two poses (the second pose is interpreted as a delta).
        /// </summary>
        /// <param name="leftSide">First pose</param>
        /// <param name="rightSide">Second pose (the delta)</param>
        /// <returns>The pose obtained by adding the two.</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Pose2D operator +(Pose2D leftSide, Pose2D rightSide)
        {
            return new Pose2D(leftSide.x + rightSide.x, leftSide.y + rightSide.y, leftSide.heading + rightSide.heading);
        }

        /// <summary>
        /// Multiply all elements of the pose by a scalar.
        /// </summary>
        /// <param name="leftSide">Input pose</param>
        /// <param name="scalar">Amount by which to multiply</param>
        /// <returns>The pose obtained by multiplying the pose by the scalar.</returns>
        public static Pose2D operator *(Pose2D leftSide, double scalar)
        {
            return new Pose2D(leftSide.x * scalar, leftSide.y * scalar, leftSide.heading * scalar);
        }

        /// <summary>
        /// Divide all elements of the pose by a scalar.
        /// </summary>
        /// <param name="leftSide">Input pose</param>
        /// <param name="scalar">Amount by which to divide</param>
        /// <returns>The pose obtained by dividing the pose by the scalar.</returns>
        public static Pose2D operator /(Pose2D leftSide, double scalar)
        {
            return new Pose2D(leftSide.x / scalar, leftSide.y / scalar, leftSide.heading / scalar);
        }

        /// <summary>
        /// Interpolate the given heading values to the desired step.
        /// It takes care of startHeading or endHeading wrapping around.
        /// The returned angle is clamped to be between -pi and pi.
        /// </summary>
        /// <param name="start">The start time for interpolation</param>
        /// <param name="end">The end time for interpolation</param>
        /// <param name="desired">Desired time for interpolation. This must be between (inclusive) start and end times</param>
        /// <param name="startHeading">Start heading value</param>
        /// <param name="endHeading">End  heading value</param>        
        /// <returns>The linear interpolated value at the desired step</returns>
        public static double LinearInterpolateHeading(long start, long end, long desired, double startHeading, double endHeading)
        {
            if (end <= start)
            {
                throw new ArgumentOutOfRangeException(string.Format("end time {0} should be greater than start time {1}", end, start));
            }

            if (start > desired || end < desired)
            {
                throw new System.ArgumentOutOfRangeException(string.Format(
                    "desiredTime {0} should be between startTime {1} and {2} inclusive",
                    desired,
                    start,
                    end));
            }

            // endtime >= starttime was already checked in constructor.
            double interpolationFactor = 1.0d * (desired - start) / (end - start);
            int sign = Math.Sign(endHeading - startHeading);
            return Pose2D.ClampAngle(startHeading + sign * SmallestCircularDeltaAngle(endHeading, startHeading) * interpolationFactor);
        }

        /// <summary>
        /// Finds the smallest angle between heading1 angle and heading2 angle.
        /// </summary>
        /// <param name="heading1">The first heading angle</param>
        /// <param name="heading2">The second heading angle</param>
        /// <returns>The heading difference</returns>
        public static double SmallestCircularDeltaAngle(double heading1, double heading2)
        {
            double clampedHeading1 = Pose2D.ClampAngle(heading1);
            double clampedHeading2 = Pose2D.ClampAngle(heading2);
            double diff1 = Math.Abs(clampedHeading1 - clampedHeading2);
            double diff2 = Math.Abs((clampedHeading1 + MathConstants.TwoPI) - clampedHeading2);
            double diff3 = Math.Abs((clampedHeading2 + MathConstants.TwoPI) - clampedHeading1);
            return Math.Min(Math.Min(diff1, diff2), diff3);
        }

        /// <summary>
        /// Finds the smallest angle between this angle and a target angle.
        /// </summary>
        /// <param name="target">The target angle</param>
        /// <returns>The heading difference</returns>
        public double SmallestCircularDeltaAngle(double target)
        {
            return Pose2D.SmallestCircularDeltaAngle(this.heading, target);
        }
        
        /// <summary>
        /// Retrieves a string representation of the Pose.
        /// </summary>
        /// <returns>String that represents the Pose.</returns>
        public override string ToString()
        {
            return string.Format("<{0}, {1}, {2}rad>", this.x, this.y, this.heading);
        }

        /// <summary>
        /// Checks for equality of two poses.
        /// </summary>
        /// <param name="obj">Pose to be compared</param>
        /// <returns>Comparison result</returns>
        public override bool Equals(object obj)
        {
            Pose2D other = (Pose2D)obj;
            return this == other;
        }

        /// <summary>
        /// Checks for equality of two poses (up to floating point errors).
        /// </summary>
        /// <param name="pose">Pose to be compared</param>
        /// <param name="epsilon">The tolerance for comparison</param>
        /// <returns>Comparison result</returns>
        public bool EqualsWithinEpsilon(Pose2D pose, double epsilon = MathConstants.ErrorEpsilon)
        {
            double headingDiff = this.heading - pose.heading;
            headingDiff = ClampAngle(headingDiff);
            return Math.Abs(this.x - pose.x) <= epsilon && Math.Abs(this.y - pose.y) <= epsilon && Math.Abs(headingDiff) <= epsilon;
        }

        /// <summary>
        /// Compute the sum of the absolute differences of the elements of two poses.
        /// </summary>
        /// <param name="pose">Pose to be compared</param>
        /// <returns>Sum of the absolute differences</returns>
        public double AbsDiffNorm(Pose2D pose)
        {
            double headingDiff = this.heading - pose.heading;
            headingDiff = ClampAngle(headingDiff);
            return Math.Abs(this.x - pose.x) + Math.Abs(this.y - pose.y) + Math.Abs(headingDiff);
        }

        /// <summary>
        /// Compute the total translation (euclidean) Square Root (x * x + y * y).
        /// </summary>
        /// <returns>Total translation</returns>
        public double TotalTranslation()
        {
            return Math.Sqrt(this.X * this.X + this.Y * this.Y);
        }

        /// <summary>
        /// Returns HashCode of object.
        /// </summary>
        /// <returns>HashCode result</returns>
        public override int GetHashCode()
        {
            return this.x.GetHashCode() ^ this.y.GetHashCode() ^ this.heading.GetHashCode();
        }
    }
}
